/*
This example shows how to take simple range measurements with three VL53L1X sensors.
The range readings are in units of mm.
*/

#include <Wire.h>
#include <VL53L1X.h>

// 定义 XSHUT 引脚
const int XSHUT_1 = 35;
const int XSHUT_2 = 36;
const int XSHUT_3 = 37;

// 定义三个传感器对象
VL53L1X sensor1;
VL53L1X sensor2;
VL53L1X sensor3;

// 定义自定义 I2C 地址
const uint8_t SENSOR1_ADDR = 0x30;
const uint8_t SENSOR2_ADDR = 0x31;
const uint8_t SENSOR3_ADDR = 0x32;

void setup() {
  Serial.begin(115200);
  Wire.begin(38,39,100000);

  // 初始化 XSHUT 引脚为输出模式并拉低
  pinMode(XSHUT_1, OUTPUT);
  pinMode(XSHUT_2, OUTPUT);
  pinMode(XSHUT_3, OUTPUT);
  digitalWrite(XSHUT_1, LOW);
  digitalWrite(XSHUT_2, LOW);
  digitalWrite(XSHUT_3, LOW);
  delay(10);

  // 初始化第一个传感器
  digitalWrite(XSHUT_1, HIGH);
  delay(10);
  sensor1.setTimeout(500);
  if (!sensor1.init()) {
    Serial.println("Failed to detect and initialize sensor 1!");
    while (1);
  }
  sensor1.setAddress(SENSOR1_ADDR);

  // 初始化第二个传感器
  digitalWrite(XSHUT_2, HIGH);
  delay(10);
  sensor2.setTimeout(500);
  if (!sensor2.init()) {
    Serial.println("Failed to detect and initialize sensor 2!");
    while (1);
  }
  sensor2.setAddress(SENSOR2_ADDR);

  // 初始化第三个传感器
  digitalWrite(XSHUT_3, HIGH);
  delay(10);
  sensor3.setTimeout(500);
  if (!sensor3.init()) {
    Serial.println("Failed to detect and initialize sensor 3!");
    while (1);
  }
  sensor3.setAddress(SENSOR3_ADDR);

  // 设置传感器参数
  sensor1.setDistanceMode(VL53L1X::Long);
  sensor1.setMeasurementTimingBudget(50000);
  sensor1.startContinuous(50);

  sensor2.setDistanceMode(VL53L1X::Long);
  sensor2.setMeasurementTimingBudget(50000);
  sensor2.startContinuous(50);

  sensor3.setDistanceMode(VL53L1X::Long);
  sensor3.setMeasurementTimingBudget(50000);
  sensor3.startContinuous(50);
}

void loop() {
  // 读取并输出第一个传感器的数据
  uint16_t range1 = sensor1.read();
  Serial.printf("%04d\t",range1);
  if (sensor1.timeoutOccurred()) {
    Serial.print(" TIMEOUT");
  }

  // 读取并输出第二个传感器的数据
  uint16_t range2 = sensor2.read();
  Serial.printf("%04d\t",range2);
  if (sensor2.timeoutOccurred()) {
    Serial.print(" TIMEOUT");
  }

  // 读取并输出第三个传感器的数据
  uint16_t range3 = sensor3.read();
  Serial.printf("%04d\t",range3);
  if (sensor3.timeoutOccurred()) {
    Serial.print(" TIMEOUT");
  }

  Serial.println();
  delay(50);
}